#!/usr/bin/python3
# -*- coding: utf-8 -*-
from joystick import JoyStick
import threading
import sys
import time

sys.path.append("../")
sys.path.append("../cart")
from cart.chassis import Chassis
from widgets import *
from config import FRONT_CAM


#本文件用于使用手柄控制车子
# 初始化底盘
cart = Chassis()

class Controller:
    def __init__(self):
        self.started = False
        self.stopped_ = False
        self.error = 0

    def start(self):
        self.started = True
        cart.steer(self.error)

    def pause(self):
        self.started = False
        cart.stop()

    def stop(self):
        if self.stopped_:
            return
        self.stopped_ = True
        cart.stop()

    def stopped(self):
        return self.stopped_


js = JoyStick()
controller = Controller()
b = Buzzer()

# 控制线程：读取摇杆输入控制车子运动
def joystick_thread():
    while not controller.stopped():
        _time, value, type_, number = js.read()
       

        # 打印所有输入事件
        input_type = js.type(type_)
        print("[Joystick Input] Time: {}, Type: {}, Number: {}, Value: {}".format(_time, input_type, number, value))


        if js.type(type_) == "button":
            # print("button:{} state: {}".format(number, value))
            if number == 6:#左扳机上面的按钮
                if value == 1:
                    controller.start()
                else: 
                    controller.pause()
                    time.sleep(0.1)
            elif number == 7 and value == 1:#左面版的上下键
                controller.stop()

        if controller.started:
            if js.type(type_) == "axis" and number == 2:#右摇杆左右摆
                # print("axis:{} state: {}".format(number, value))
                controller.error = value * 1.0 / 32767
                cart.steer(controller.error)
        else:
            cart.stop()

    cart.stop()


def main():
    js.open()

    while True:
        # 蜂鸣器提示准备就绪
        b.rings()
        time.sleep(0.2)
        b.rings()
        time.sleep(0.1)
        b.rings()
        time.sleep(0.1)

        controller.error = 0
        controller.stopped_ = False
        cart.speed = 20

        # 启动手柄控制线程
        t = threading.Thread(target=joystick_thread, args=())
        t.start()
        t.join()

        # 等待下一轮开始/退出
        while True:
            _time, value, type_, number = js.read()
            if number == 11 and value == 1:
                break
            elif number == 10 and value == 1:
                return 0


if __name__ == "__main__":
    main()
    for i in range(3):
        b.rings()
        time.sleep(0.2)
